//testing 2 VL53L1X on Final Project board

//Call necessary libraries
#include <Wire.h>
#include <VL53L1X.h>

//Define Sensors
VL53L1X Left;
//VL53L1X Middle;
VL53L1X Right;
//VL53L1X Extra;

//Define ATtiny3216 pin variables
//VCC on pin 1
int PWMA = 0;   //PWMA on pin 2
int PWMB = 1;   //PWMB on pin 3
int INA1 = 2;   //INA1 on pin 4
int INA2 = 3;   //INA2 on pin 5
int INB1 = 4;   //INB1 on pin 6
int INB2 = 5;   //INB2 on pin 7
//ATtiny3216 RX on pin 8 (6 Arduino designation)
//ATtiny3216 TX on pin 9 (7 Arduino designation)
//ATtiny3216 SDA on pin 10 (8 Arduino designation) 
//ATtiny3216 SCL on pin 11 (9 Arduino designation) 
int XSHUT_L = 10;   //Left XSHUT on pin 12
int XSHUT_M = 11;   //Middle XSHUT on pin 13
int XSHUT_R = 12;   //Right XSHUT on pin 14
int XSHUT_E = 13;   //Extra XSHUT on pin 15
//pin 16 (17 Arduino designation) is the UPDI pin
//pin 17 (14 Arduino designaion)is not connected to anything
//pin 18 (15 Arduino designaion)is not connected to anything
int LED = 16;   //LED on pin 19
//GND on pin 20

void setup() {
  //setup pin modes
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(INA1, OUTPUT);
  pinMode(INA2, OUTPUT);
  pinMode(INB1, OUTPUT);
  pinMode(INB2, OUTPUT);
  pinMode(XSHUT_L, OUTPUT);
  //pinMode(XSHUT_M, OUTPUT);
  pinMode(XSHUT_R, OUTPUT);
  //pinMode(XSHUT_E, OUTPUT);
  pinMode(LED, OUTPUT);

  //set all XSHUT pins to LOW
  digitalWrite(XSHUT_L, LOW);
  //digitalWrite(XSHUT_M, LOW);
  digitalWrite(XSHUT_R, LOW);
  //digitalWrite(XSHUT_E, LOW);
  
  //Initialize I2C
  delay(500);
  Wire.begin();
  Wire.beginTransmission(0x29);
  Wire.setClock(400000);  //use 400 kHz for I2C
  Serial.begin(115200);

  //set address of left sensor
  digitalWrite(XSHUT_L, HIGH);
  delay(150);
  Serial.println("00");
  Left.init();
  Serial.println("01");
  delay(100);
  Left.setAddress(0x31);
  Serial.println("02");

//  //set address of middle Sensor
//  digitalWrite(XSHUT_M, HIGH);
//  delay(150);
//  Serial.println("03");
//  Middle.init();
//  Serial.println("04");
//  delay(100);
//  Middle.setAddress(0x33);
//  Serial.println("05");
  
  //set address of middle Sensor
  digitalWrite(XSHUT_R, HIGH);
  delay(150);
  Serial.println("06");
  Right.init();
  Serial.println("07");
  delay(100);
  Right.setAddress(0x35);
  Serial.println("08");
  
//  //set address of extra Sensor
//  digitalWrite(XSHUT_E, HIGH);
//  delay(150);
//  Serial.println("09");
//  Extra.init();
//  Serial.println("10");
//  delay(100);
//  Extra.setAddress(0x37);
//  Serial.println("11");

  //setup Left sensor
  Left.setDistanceMode(VL53L1X::Long);
  Left.setMeasurementTimingBudget(50000);
  Left.startContinuous(50);
  Left.setTimeout(100);
  
//  //setup Middle sensor
//  Middle.setDistanceMode(VL53L1X::Long);
//  Middle.setMeasurementTimingBudget(50000);
//  Middle.startContinuous(50);
//  Middle.setTimeout(100);
  
  //setup Right sensor
  Right.setDistanceMode(VL53L1X::Long);
  Right.setMeasurementTimingBudget(50000);
  Right.startContinuous(50);
  Right.setTimeout(100);

//  //setup Extra sensor
//  Extra.setDistanceMode(VL53L1X::Long);
//  Extra.setMeasurementTimingBudget(50000);
//  Extra.startContinuous(50);
//  Extra.setTimeout(100);

  delay(150);
  Serial.println("addresses set");

  Serial.println("I2C scanner. Scanning ...");
  byte count = 0;
  for (byte i = 1; i < 120; i++)  {
    Wire.beginTransmission (i);
    if (Wire.endTransmission () == 0) {
      Serial.print("Found address: ");
      Serial.print(i, DEC);
      Serial.print(" (0x");
      Serial.print(i, HEX);
      Serial.println(")");
      count++;
      delay(1);
    } //end of good response
  } //end of for loop
  Serial.println("Done.");
  Serial.print("Found ");
  Serial.print(count, DEC);
  Serial.println(" device(s).");

  digitalWrite(LED, HIGH);
} //end of void setup()

void loop() {
  //read Left Sensor
  Serial.print("Left sensor: ");
  Serial.print(Left.read());
  if (Left.timeoutOccurred()) {Serial.print("Timeout");}
  Serial.println();
  delay(500);
//  //read Middle Sensor
//  Serial.print("Middle sensor: ");
//  Serial.print(Middle.read());
//  if (Middle.timeoutOccurred()) {Serial.print("Timeout");}
//  Serial.println();
//  delay(500);
  //read Right Sensor
  Serial.print("Right sensor: ");
  Serial.print(Right.read());
  if (Right.timeoutOccurred()) {Serial.print("Timeout");}
  Serial.println();
  delay(500);
//  //read Extra Sensor
//  Serial.print("Extra sensor: ");
//  Serial.print(Extra.read());
//  if (Extra.timeoutOccurred()) {Serial.print("Timeout");}
//  Serial.println();
//  delay(500);
} //end of void loop()